#include "mlx90640.h"
#include <Adafruit_MLX90640.h>
#include "SPI.h"
#include "interpolation.h"
extern "C"
{
#include "LCD.h"
#include "GUI.h"
}

Adafruit_MLX90640 mlx;

// uncomment *one* of the below
// #define PRINT_TEMPERATURES
#define PRINT_ASCIIART
// low range of the sensor (this will be blue on the screen)
#define MINTEMP 15
// high range of the sensor (this will be red on the screen)
#define MAXTEMP 37
#define MLX_COLS 32
#define MLX_ROWS 24

// #define INTERPOLATED_COLS LCD_W / MLX_COLS * LCD_W
// #define INTERPOLATED_ROWS LCD_H / MLX_ROWS * LCD_H
// 插值倍率
// 采用混合插值，从32*24三倍插值到96*72，再通过像素212方式像素复制到160*120
#define INTERPOLATED_COLS MLX_COLS * 2
#define INTERPOLATED_ROWS MLX_ROWS * 2

//数据buffer
float frame[MLX_COLS * MLX_ROWS]; // buffer for full frame of temperatures
uint16_t displayPixelWidth, displayPixelHeight;
uint16_t displayPixelX, displayPixelY;
//插值后的buffer
// float dest_2d[INTERPOLATED_ROWS * INTERPOLATED_COLS];
float dest_2d[INTERPOLATED_ROWS * INTERPOLATED_COLS];
const uint16_t camColors[] = {0x480F,
0x400F,0x400F,0x400F,0x4010,0x3810,0x3810,0x3810,0x3810,0x3010,0x3010,
0x3010,0x2810,0x2810,0x2810,0x2810,0x2010,0x2010,0x2010,0x1810,0x1810,
0x1811,0x1811,0x1011,0x1011,0x1011,0x0811,0x0811,0x0811,0x0011,0x0011,
0x0011,0x0011,0x0011,0x0031,0x0031,0x0051,0x0072,0x0072,0x0092,0x00B2,
0x00B2,0x00D2,0x00F2,0x00F2,0x0112,0x0132,0x0152,0x0152,0x0172,0x0192,
0x0192,0x01B2,0x01D2,0x01F3,0x01F3,0x0213,0x0233,0x0253,0x0253,0x0273,
0x0293,0x02B3,0x02D3,0x02D3,0x02F3,0x0313,0x0333,0x0333,0x0353,0x0373,
0x0394,0x03B4,0x03D4,0x03D4,0x03F4,0x0414,0x0434,0x0454,0x0474,0x0474,
0x0494,0x04B4,0x04D4,0x04F4,0x0514,0x0534,0x0534,0x0554,0x0554,0x0574,
0x0574,0x0573,0x0573,0x0573,0x0572,0x0572,0x0572,0x0571,0x0591,0x0591,
0x0590,0x0590,0x058F,0x058F,0x058F,0x058E,0x05AE,0x05AE,0x05AD,0x05AD,
0x05AD,0x05AC,0x05AC,0x05AB,0x05CB,0x05CB,0x05CA,0x05CA,0x05CA,0x05C9,
0x05C9,0x05C8,0x05E8,0x05E8,0x05E7,0x05E7,0x05E6,0x05E6,0x05E6,0x05E5,
0x05E5,0x0604,0x0604,0x0604,0x0603,0x0603,0x0602,0x0602,0x0601,0x0621,
0x0621,0x0620,0x0620,0x0620,0x0620,0x0E20,0x0E20,0x0E40,0x1640,0x1640,
0x1E40,0x1E40,0x2640,0x2640,0x2E40,0x2E60,0x3660,0x3660,0x3E60,0x3E60,
0x3E60,0x4660,0x4660,0x4E60,0x4E80,0x5680,0x5680,0x5E80,0x5E80,0x6680,
0x6680,0x6E80,0x6EA0,0x76A0,0x76A0,0x7EA0,0x7EA0,0x86A0,0x86A0,0x8EA0,
0x8EC0,0x96C0,0x96C0,0x9EC0,0x9EC0,0xA6C0,0xAEC0,0xAEC0,0xB6E0,0xB6E0,
0xBEE0,0xBEE0,0xC6E0,0xC6E0,0xCEE0,0xCEE0,0xD6E0,0xD700,0xDF00,0xDEE0,
0xDEC0,0xDEA0,0xDE80,0xDE80,0xE660,0xE640,0xE620,0xE600,0xE5E0,0xE5C0,
0xE5A0,0xE580,0xE560,0xE540,0xE520,0xE500,0xE4E0,0xE4C0,0xE4A0,0xE480,
0xE460,0xEC40,0xEC20,0xEC00,0xEBE0,0xEBC0,0xEBA0,0xEB80,0xEB60,0xEB40,
0xEB20,0xEB00,0xEAE0,0xEAC0,0xEAA0,0xEA80,0xEA60,0xEA40,0xF220,0xF200,
0xF1E0,0xF1C0,0xF1A0,0xF180,0xF160,0xF140,0xF100,0xF0E0,0xF0C0,0xF0A0,
0xF080,0xF060,0xF040,0xF020,0xF800,};

void mlxInit()
{
    Wire.setPins(14, 15);
    Serial.println("Adafruit MLX90640 Simple Test");
    if (!mlx.begin(MLX90640_I2CADDR_DEFAULT, &Wire))
    {
        Serial.println("MLX90640 not found!");
        return;
    }
    Serial.println("Found Adafruit MLX90640");

    // displayPixelWidth = tft.height() / 8;
    // displayPixelHeight = tft.height() / 8;
    // displayPixelX = (tft.width() - displayPixelWidth * 8) / 2;
    // displayPixelY = (tft.height() - displayPixelHeight * 8) / 2;

    // drawTemperateBar();

    Serial.print("Serial number: ");
    Serial.print(mlx.serialNumber[0], HEX);
    Serial.print(mlx.serialNumber[1], HEX);
    Serial.println(mlx.serialNumber[2], HEX);

    // mlx.setMode(MLX90640_INTERLEAVED);
    mlx.setMode(MLX90640_CHESS);
    Serial.print("Current mode: ");
    if (mlx.getMode() == MLX90640_CHESS)
    {
        Serial.println("Chess");
    }
    else
    {
        Serial.println("Interleave");
    }

    mlx.setResolution(MLX90640_ADC_18BIT);
    Serial.print("Current resolution: ");
    mlx90640_resolution_t res = mlx.getResolution();
    switch (res)
    {
    case MLX90640_ADC_16BIT:
        Serial.println("16 bit");
        break;
    case MLX90640_ADC_17BIT:
        Serial.println("17 bit");
        break;
    case MLX90640_ADC_18BIT:
        Serial.println("18 bit");
        break;
    case MLX90640_ADC_19BIT:
        Serial.println("19 bit");
        break;
    }

    mlx.setRefreshRate(MLX90640_4_HZ);
    Serial.print("Current frame rate: ");
    mlx90640_refreshrate_t rate = mlx.getRefreshRate();
    switch (rate)
    {
    case MLX90640_0_5_HZ:
        Serial.println("0.5 Hz");
        break;
    case MLX90640_1_HZ:
        Serial.println("1 Hz");
        break;
    case MLX90640_2_HZ:
        Serial.println("2 Hz");
        break;
    case MLX90640_4_HZ:
        Serial.println("4 Hz");
        break;
    case MLX90640_8_HZ:
        Serial.println("8 Hz");
        break;
    case MLX90640_16_HZ:
        Serial.println("16 Hz");
        break;
    case MLX90640_32_HZ:
        Serial.println("32 Hz");
        break;
    case MLX90640_64_HZ:
        Serial.println("64 Hz");
        break;
    }
}

void mlxTest()
{

    Serial.println("Failed");
    if (mlx.getFrame(frame) != 0)
    {
        Serial.println("Failed");
        return;
    }
    Serial.println();
    Serial.println();
    for (uint8_t h = 0; h < 24; h++)
    {
        for (uint8_t w = 0; w < 32; w++)
        {
            float t = frame[h * 32 + w];
#ifdef PRINT_TEMPERATURES
            Serial.print(t, 1);
            Serial.print(", ");
#endif
#ifdef PRINT_ASCIIART
            char c = '&';
            if (t < 20)
                c = ' ';
            else if (t < 23)
                c = '.';
            else if (t < 25)
                c = '-';
            else if (t < 27)
                c = '*';
            else if (t < 29)
                c = '+';
            else if (t < 31)
                c = 'x';
            else if (t < 33)
                c = '%';
            else if (t < 35)
                c = '#';
            else if (t < 37)
                c = 'X';
            Serial.print(" ");
            Serial.print(c);
            Serial.print(" ");
#endif
        }
        Serial.println();
    }
}

/* 画温度颜色对比条 */
void mlxDrawTemperateBar()
{
    // const int colorSize = 256;
    // const int barHeight = 128;
    // const int barWidth = 10;
    // const int posY = (tft.height() - barHeight) / 2 - 1;
    // for (int i = 0; i < colorSize; i++)
    // {
    //     int x = i / barHeight;
    //     tft.drawLine(14 + x * barWidth, barHeight - i % barHeight + posY, 14 + x * barWidth + barWidth, barHeight - i % barHeight + posY, camColors[i]);
    // }

    // tft.setTextSize(1);

    // tft.setTextDatum(BL_DATUM);
    // tft.setTextColor(camColors[0]);
    // tft.drawString(String(MINTEMP), 0, tft.height());

    // tft.setTextColor(camColors[barHeight]);
    // tft.drawString(String(MINTEMP + int(MAXTEMP - MINTEMP) / 2), 38, tft.height());

    // tft.setTextDatum(TL_DATUM);
    // tft.setTextColor(camColors[barHeight - 1]);
    // tft.drawString(String(MINTEMP + int(MAXTEMP - MINTEMP) / 2), 0, 0);

    // tft.setTextColor(camColors[colorSize - 1]);
    // tft.drawString(String(MAXTEMP), 38, 0);
}

/* 画热成像素点 */
void mlxDrawpixels(float *p, uint8_t rows, uint8_t cols, boolean showVal)
{
    int colorTemp;
    int posX=0,posY=0,tempX,tempY;
    float curMax = get_point(p, rows, cols, 0, 0),
          curMin = get_point(p, rows, cols, 0, 0),
          curMid = get_point(p, rows, cols, cols / 2, rows / 2);
    uint16_t colorMax = camColors[0],
             colorMin = camColors[0],
             colorMid = camColors[0];
    // 画热成像
    for (int y = 0; y < rows; y++)
    {
        tempY=y%2+2;
        posY+=tempY;
        posX=0;
        for (int x = 0; x < cols; x++)
        {
            tempX=x%2+2;
            posX+=tempX;
            float val = get_point(p, rows, cols, x, y);

            if (val >= MAXTEMP)
                colorTemp = MAXTEMP;
            else if (val <= MINTEMP)
                colorTemp = MINTEMP;
            else
                colorTemp = val;

            uint8_t colorIndex = map(colorTemp, MINTEMP, MAXTEMP, 0, 255);
            colorIndex = constrain(colorIndex, 0, 255);
            // draw the pixels!
            // GUI_DrawPoint(cols-x,y,camColors[colorIndex]);
            
            
            // 
            // 
            // Serial.printf("posX:%d,posY:%d\r\n",posX,posY);

            LCD_Fill(posX-tempX,posY-tempY,posX,posY,camColors[colorIndex]);
            // LCD_Fill(x,y,x,y,camColors[colorIndex]);
            // tft.fillRect(displayPixelX + boxWidth * x, displayPixelY + boxHeight * y, boxWidth, boxHeight, camColors[colorIndex]);

            // 取最大值
            if (val >= curMax)
            {
                curMax = val;
                colorMax = camColors[colorIndex];
            }
            // 取最小值
            if (val <= curMin)
            {
                curMin = val;
                colorMin = camColors[colorIndex];
            }
            // 取中心点
            if (y == rows / 2 && x == cols / 2)
            {
                colorMid = camColors[colorIndex];
            }

            // if (showVal)
            // {
            //     tft.setCursor(boxWidth * y + boxWidth / 2 - 12, 40 + boxHeight * x + boxHeight / 2 - 4);
            //     tft.setTextSize(1);
            //     tft.setTextColor(TFT_WHITE);
            //     tft.print(val, 1);
            // }
        }
    }
    // // 画中心点十字
    // uint8_t midX = displayPixelX + cols * boxWidth / 2;
    // uint8_t midY = displayPixelY + rows * boxHeight / 2;
    // midY = midY + (tft.height() / 8);
    // tft.drawLine(midX - 10, midY, midX + 10, midY, TFT_WHITE);
    // tft.drawLine(midX, midY - 10, midX, midY + 10, TFT_WHITE);

    // char temp[7];
    // tft.setTextSize(1);
    // tft.setTextColor(TFT_WHITE);
    // // 显示最高温度
    // tft.setTextDatum(TL_DATUM);
    // tft.drawString(F("MAX"), tft.width() - 40, 0);
    // tft.fillRect(tft.width() - 45, 10, 40, 30, colorMax);
    // tft.setTextDatum(CL_DATUM);
    // memset(temp, 0, sizeof(temp));
    // sprintf(temp, "%0.1f", curMax);
    // tft.drawString(String(temp), tft.width() - 40, 25);
    // // 显示最低温度
    // tft.setTextDatum(TL_DATUM);
    // tft.drawString(F("MIN"), tft.width() - 40, 47);
    // tft.fillRect(tft.width() - 45, 57, 40, 30, colorMin);
    // tft.setTextDatum(CL_DATUM);
    // memset(temp, 0, sizeof(temp));
    // sprintf(temp, "%0.1f", curMin);
    // tft.drawString(String(temp), tft.width() - 40, 72);
    // // 显示中心点温度
    // tft.setTextDatum(TL_DATUM);
    // tft.drawString(F("POINT"), tft.width() - 40, 94);
    // tft.fillRect(tft.width() - 45, 104, 40, 30, colorMid);
    // tft.setTextDatum(CL_DATUM);
    // memset(temp, 0, sizeof(temp));
    // sprintf(temp, "%0.1f", curMid);
    // tft.drawString(String(temp), tft.width() - 40, 119);

}

void mlxRefresh()
{
    if (mlx.getFrame(frame) != 0)
    {
        Serial.println("mlx read failed");
        return;
    }

 int32_t t = millis();
    int colorTemp;
for (int y = 0; y < MLX_ROWS; y++)
    {
        for (int x = 0; x < MLX_COLS; x++)
        {
            float val = get_point(frame, MLX_ROWS, MLX_COLS, x, y);

            if (val >= MAXTEMP)
                colorTemp = MAXTEMP;
            else if (val <= MINTEMP)
                colorTemp = MINTEMP;
            else
                colorTemp = val;

            uint8_t colorIndex = map(colorTemp, MINTEMP, MAXTEMP, 0, 255);
            colorIndex = constrain(colorIndex, 0, 255);
            // draw the pixels!
            GUI_DrawPoint(MLX_COLS-x,y,camColors[colorIndex]);
            
            
          
        }
    }
        Serial.printf("drawpixels times:%d\r\n",millis()-t);
    // int32_t t = millis();
    // interpolate_image(frame, MLX_ROWS, MLX_COLS, dest_2d, INTERPOLATED_ROWS, INTERPOLATED_COLS);
    // Serial.printf("interpolate times:%d\r\n",millis()-t);
    // t = millis();
    // mlxDrawpixels(dest_2d, INTERPOLATED_ROWS, INTERPOLATED_COLS,false);
    // Serial.printf("drawpixels times:%d\r\n",millis()-t);
}